#简化时间
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import threading
import time
from datetime import datetime, timedelta

import paho.mqtt.client as mqtt

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

from my_custom_msgs.msg import TrajectoryInfo, TimeInfo


class MQTTPublisher(Node):
    def __init__(self):
        super().__init__('mqtt_publisher')

        # 共享状态
        self._lock = threading.Lock()
        self.current_trajectory = 0
        self.total_executions = 0
        self.current_time_str = ""
        self.elapsed_time_str = ""
        self.last_time_info_at: datetime | None = None

        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
        )

        # 订阅
        self.create_subscription(TrajectoryInfo, 'trajectory_info', self.trajectory_cb, qos)
        self.create_subscription(TimeInfo, 'time_info', self.time_info_cb, qos)

        # ====== MQTT 配置（xyyl配置）======
        self.broker = "cscserver.cn"
        self.port = 1883
        self.topic = "/iot/KT4201/realtimeData"
        self.username = "vipxyyl"
        self.password = "vipxyyl"
        self.client_id = "KT4201"

        self.mqtt_client = mqtt.Client(self.client_id)
        self.mqtt_client.username_pw_set(self.username, self.password)

        def on_connect(client, userdata, flags, rc):
            if rc == 0:
                self.get_logger().info("MQTT 连接成功")
            else:
                self.get_logger().error(f"MQTT 连接失败，返回码: {rc}")

        self.mqtt_client.on_connect = on_connect
        self.mqtt_client.connect(self.broker, self.port, keepalive=60)
        self.mqtt_client.loop_start()

        # 定时器：3 秒一次
        self.timer = self.create_timer(3.0, self.publish_tick)

        self.get_logger().info("MQTTPublisher (ROS2) 已启动。")

    def trajectory_cb(self, msg: TrajectoryInfo):
        with self._lock:
            self.current_trajectory = int(msg.current_trajectory)
            self.total_executions = int(msg.total_executions)

    def time_info_cb(self, msg: TimeInfo):
        with self._lock:
            self.current_time_str = str(msg.current_time).strip()   # 固定格式: YYYYMMDDHHMMSS
            self.elapsed_time_str = str(msg.elapsed_time).strip()   # 格式: HH:MM:SS
            self.last_time_info_at = datetime.now()

    def publish_tick(self):
        try:
            with self._lock:
                current_trajectory = self.current_trajectory
                total_executions = self.total_executions
                current_time = self.current_time_str
                elapsed_time = self.elapsed_time_str
                last_ts = self.last_time_info_at

            online = (
                last_ts is not None
                and (datetime.now() - last_ts) <= timedelta(seconds=5)
                and current_time and elapsed_time
            )

            if online:
                try:
                    ct = datetime.strptime(current_time, "%Y%m%d%H%M%S")
                    year, month, day = ct.year % 100, ct.month, ct.day
                    hour, minute = ct.hour, ct.minute

                    et = datetime.strptime(elapsed_time, "%H:%M:%S")
                    hours, minutes, seconds = et.hour, et.minute, et.second
                except Exception as e:
                    self.get_logger().warn(f"时间解析失败：{e}，发送默认数据")
                    online = False

            if online:
                data = bytearray([
                    current_trajectory & 0xFF,
                    total_executions & 0xFF,
                    year, month, day, hour, minute,
                    hours & 0xFF, minutes & 0xFF, seconds & 0xFF,
                    0xcd, 0xdc
                ])
                header_hex = "0g02"
                full_message = header_hex + data.hex()
            else:
                data = bytearray([
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x00, 0x00,
                    0xcd, 0xdc
                ])
                header_hex = "0g02"
                full_message = header_hex + data.hex()
                self.get_logger().warn("设备离线……发送默认数据")

            self.mqtt_client.publish(self.topic, payload=full_message, qos=0, retain=False)
            self.get_logger().info(f"已发送数据: {full_message}")

        except Exception as e:
            self.get_logger().error(f"发布异常：{e}")

    def destroy_node(self):
        try:
            self.mqtt_client.loop_stop()
            self.mqtt_client.disconnect()
        except Exception:
            pass
        super().destroy_node()


def main():
    rclpy.init()
    node = MQTTPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("收到中断信号，准备退出...")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
